/**
@author : MAMAMA
@date: 2019-07-12
@description: control panasonic dc motor
*/

#include "Kinco.h"
#include <math.h>

/* Local Functions */
void modbus_delay(uint32_t t)
{
    delay_ms(t);
}


/* Global Variables --------------------------------------------------*/

/**
  * @brief  电机modbus初始化
  * @param  axisid: 轴编号（modbus地址），pstream: 串口通信类地址，pmodbus: modbus节点类地址
  * @retval None
  */
KincoMotor::KincoMotor(uint8_t axisid, Stream *pstream) : _pstream(pstream),
                                                                  axis_id(axisid)
{
    _pmodbus_master = new ModbusMaster();
    _connect_state = OFFLINE;
    _vel_cmd_val = 0;
    status = 0x00;
}

KincoMotor::~KincoMotor(void)
{
    delete _pmodbus_master;
}

bool KincoMotor::connectOnce(void)
{
    bool ret = false;
    uint8_t retcode;
    uint16_t cnt = 0;
    
    if(status==0x00)
    {
        retcode = _pmodbus_master->readHoldingRegisters(STATUS_WORD, 1);
        modbus_delay(MODBUS_INTERVAL);
        if (retcode == 0)
        {
            status = _pmodbus_master->getResponseBuffer(0);
            _pmodbus_master->modbus_error_cnt = 0;  //连上之后清除错误计数
            _connect_state = ONLINE;
            ret = true;
        }
    }
    else
        ret = true;
    
    if(_pmodbus_master->modbus_error_cnt > 100)
    {
        _connect_state = OFFLINE;
        status = 0x00;
        ret = false;
    }
    
    return ret;
}

/**
@description send a modbus request to the motor,
recieving the correct responce means connecting success.              
@return true: connecting success
        false: connecting failure
*/
bool KincoMotor::initModbus(void)
{
    bool ret = false;
    if (_connect_state == OFFLINE)
    {
        _pmodbus_master->begin(axis_id, *_pstream);
        /**
        do some modbus task here
        */
        if(this->connectOnce())
        {
            ret = true;
            _pmodbus_master->modbus_error_cnt = 0;
            _connect_state = ONLINE;
        }
        return ret;
    }
    else
    {
        return true;
    }
}

bool KincoMotor::powerOnOff(bool on_off)
{
    uint8_t retcode = 0xFF;
    static uint16_t cw = 0;
    if(on_off)cw = KINCO_POWERON;
    else cw = KINCO_POWEROFF;
    
    if(_connect_state == ONLINE)
    {
        _pmodbus_master->clearTransmitBuffer();
        _pmodbus_master->setTransmitBuffer(0, cw);
        retcode = _pmodbus_master->writeSingleRegister(CONTROL_WORD, cw);
        modbus_delay(MODBUS_INTERVAL);
    }
    
    if (retcode == 0)
        return true;
    else
        return false;
}

bool KincoMotor::setVeloctiy(short vel)
{
    uint8_t retcode = 0xFF;
    static KincoVel_T i32vel_ = {0};
    i32vel_.vel = (int32_t)(vel * 512 * 10000.0 / 1875);
    if (_connect_state == ONLINE)
    {
        if (vel < _max_velocity && vel > (0 - _max_velocity))
        {
            _pmodbus_master->clearTransmitBuffer();
            _pmodbus_master->setTransmitBuffer(0, i32vel_.words.low);
            _pmodbus_master->setTransmitBuffer(1, i32vel_.words.high);
            retcode = _pmodbus_master->writeMultipleRegisters(TARGET_SPEED, 2);
            modbus_delay(MODBUS_INTERVAL);
            _vel_cmd_val = vel;
        }
        else
        {
            retcode = 0xFF;
        }
    }
    if (retcode == 0)
        return true;
    else
        return false;
}

/**
@description get actual motor velocity from modbus reg
@return -32768: reading error 
*/
double KincoMotor::getMotorActualVel(void)
{
    KincoVel_T vel = {0};
    double ret = -32768.0;
    double dret = 0.0;
    uint8_t retcode;
    if (_connect_state == ONLINE)
    {
        retcode = _pmodbus_master->readHoldingRegisters(ACTUAL_VEL, 2);
        if (retcode == 0)
        {
            vel.words.low = _pmodbus_master->getResponseBuffer(0);
            vel.words.high = _pmodbus_master->getResponseBuffer(1);
        }
        modbus_delay(MODBUS_INTERVAL);
        
        dret = (double)vel.vel * 1875.0 / EncoderResolution / 512;
        if(abs(dret)<5000.0)ret = dret;
    }
    return ret;
}

bool KincoMotor::getMotorActualPos(void)
{
    KincoPos_T pos = {0};
    bool ret = false;
    uint8_t retcode;
    if (_connect_state == ONLINE)
    {
        retcode = _pmodbus_master->readHoldingRegisters(ACTUAL_POS, 2);
        if (retcode == 0)
        {
            pos.words.low = _pmodbus_master->getResponseBuffer(0);
            pos.words.high = _pmodbus_master->getResponseBuffer(1);
            ret = true;
            actual_pos = pos.pos;
        }
        modbus_delay(MODBUS_INTERVAL);
    }
    return ret;
}

/**
* @brief check if alarm is happening
*/
uint8_t KincoMotor::getAlarmState(void)
{
    uint8_t retcode;
    uint8_t alm_state = 0x0;
    if (_connect_state == ONLINE)
    {
        retcode = _pmodbus_master->readCoils(MODBUS_ALM_STATE, 1);
        if (retcode == 0)
        {
            alm_state = _pmodbus_master->getResponseBuffer(0);
        }
        modbus_delay(MODBUS_INTERVAL);
    }
    return alm_state;
}

uint8_t KincoMotor::clearAlarm(void)
{
    uint8_t retcode;
    if (_connect_state == ONLINE)
    {
        _pmodbus_master->clearTransmitBuffer();
        _pmodbus_master->setTransmitBuffer(0, CLEARALARM_CMD);
        retcode = _pmodbus_master->writeMultipleRegisters(CLEARALARM, 1);
        modbus_delay(MODBUS_INTERVAL);
    }
    return retcode;
}

uint16_t KincoMotor::getTorqueLimit(void)
{
    uint8_t retcode;
    uint16_t limit = 0x0;
    //
    return limit;
}

bool KincoMotor::setTorqueLimit(unsigned short limit)
{
    uint8_t retcode;
    //
    if(retcode==0)
        return true;
    else
        return false;
}
